#include <vector>
#include <math.h>
#include <cmath>
#include <iostream>

using namespace std;

// define the parameters of the curve
struct CurveParam
{
    double aa_max;  // the maximum accelerattion acceleration
    double acc_max; // the maximum acceleration
    double t_acc;   // the period in which acceleration is increasing
    double t_keep;  // the period in which acceleration remains unchanged, but not zero 
    double t_const; // the period in which acceleration is zero

    inline bool operator==(const CurveParam &c_param) const
    {
        return (c_param.aa_max == aa_max && c_param.t_acc == t_acc && c_param.t_keep == t_keep && c_param.t_const == t_const);
    }
};

// define the parameters of the input
struct InParam
{
    double acc_max;     // the maximum acceleration
    double vel_max;     // the maximum velocity
    double aa_max;      // the maximum accelerattion acceleration

    double theta_start;     // the starting angle
    double theta_end;       // the endding angle

    inline bool operator==(const InParam &in_param) const
    {
        return (in_param.acc_max == acc_max && in_param.vel_max == vel_max && in_param.aa_max == aa_max && in_param.acc_max == acc_max && in_param.theta_start == theta_start && in_param.theta_end == theta_end);
    }
};

void Trapezoidal(InParam in_param, CurveParam &c_param);

double compute(double time, CurveParam c_param);

double TimeTool(double time, double t_1, double t_2);

// compute the relative time in each section of the curve
// input: time - the time needed to be calculated
//        t_1 - the lower bound of the section
//        t_2 - the upper bound of the section
// output: ret - the relative time in the section
// description: if time > t_2, then return the time of the whole setion,
//              if t_2 > time > t_1, then return (time - t_1)
//              if t_1 > time, then time has not reach the section, thus return 0

// compute the rotation angle given time and the curve
// input: time - the time when the angle needed to be computed
//        c_param - the parameters given by the curve
// output: ret - the rotation angle, which is integrated over all the sections
// description: when the parameters of the curve is determined, then the seven moment (t1,...,t7) on the curve is determined
//              then wo calculate the relative time in each section and then the rotation angle in each section
